#include <CParticle.h>

CParticle::CParticle()
    {
      P = 0;
      V = 0;
      A = 0;
      Mass = 1;
      Charge = 0;
      next = prev = 0;
      nn = pp = 0;
    }

CVector CParticle::Mv()
{
  return (V*Mass);
}

double CParticle::Ekin()
{
  double v = V.Length();
  return 0.5 * abs(Mass) * v*v;
}

void CParticle::Move(double deltat)
{
  V = V + A*deltat;
  P = P + V*deltat;
  A.Set(0,0,0);
}

void CParticle::LeapFrog(double deltat)
{
  P = P + V*deltat + A*deltat*deltat/2.0; 
  V = V + (A+A_old)*deltat*0.5;
  A_old = A;
  A.Set(0,0,0);
  
}




void CParticle::RungeKuttaIntegrator(float DT, double scale)
{
  CVector k0[2]; CVector k1[2]; CVector k2[2]; CVector k3[2]; CVector k4[2]; // remember all should be initiliazed zero

    RK4Eval(    0.0f, k0, k1);
    RK4Eval( DT*0.5f, k1, k2);
    RK4Eval( DT*0.5f, k2, k3);
    RK4Eval( DT     , k3, k4);
     
    CVector dxdt = (k1[0] + (k2[0]+k3[0])*2.0 + k4[0] )*(1.0f/6.0f);
    CVector dvdt = (k1[1] + (k2[1]+k3[1])*2.0 + k4[1] )*(1.0f/6.0f);
 
    P = P + dxdt * DT;
    V = V + dvdt * DT;

    A = A*0;

    /*  V = V + A;
	P = P + V;*/

    
}

void CParticle::RK4Eval(float DT, CVector d[2], CVector* output)
{
    const int x = 0; 
    const int v = 1;   // otherwise I keep thinking arrays
    CVector state[2];
    state[x] = P + d[x]*DT;
    state[v] = V + d[v]*DT;
     
    output[x] = state[v];
    output[v] = A;
}



